/*=+--+=#=+--     Unmanned Aerial System Management Software      --+=#=+--+=#*\
|          Copyright (C) 2011 Regents of the University of Colorado.           |
|                             All Rights Reserved.                             |

     This program is free software: you can redistribute it and/or modify
     it under the terms of the GNU General Public License version 2 as
     published by the Free Software Foundation.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program.  If not, see <http://www.gnu.org/licenses/>.

            Jack Elston                       Cory Dixon                        
|           elstonj@colorado.edu              dixonc@colorado.edu              |
|                                                                              |
\*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*/

/*!
  \file stargazer.h
	\brief header file for stargazer

                    \author Maciej Stachura
             $Date: 2006/10/25 13:34:45 $
*/

#ifndef __STARGAZERPARSER_H
#define __STARGAZERPARSER_H

#include "threadedPipe.h"
#include "ukf.h"
#include "matrix.h"

#define SG_TELEMETRY_PERIOD 0.5 //seconds
#define SIZE_PDATA 9 //seconds
#define PSEUDO_MEA_BUF_SIZE 68 //seconds
#define VEL_EST 0.4
#define BAD_MEAS 0.05

typedef struct{
	int ID;
	float x;
	float y;
	float angle;
}PData_t;

class StargazerParser : public ThreadedPipe
{
	private:
		double ang_corr, t_last;
    GPSTelem_t gps_r;
    GPSTelem_t gps_f;
    PData_t p_pos[SIZE_PDATA];
		uint8_t buf_ptr;
		uint8_t buf[PSEUDO_MEA_BUF_SIZE];
    Matrix z_od, z_star, z_last, R_star, R_od; // The measurement vectors
    timespec t;
    volatile float ti, tf;
    bool od_start, good_star, init_od;

		Matrix (*f_ptr)(Matrix, FiltData_t);

		TimeStamp last_telem_tx;

		SCISensorPacket_t roomba_sensors;
		void parseInternalData(const Command &cmd);

		String log_name;
		void setSharedMem(void);

	public:
		StargazerParser(String name);
		~StargazerParser() {cout << "Stargazer Destroyed\n";}

    UKF filt, filt_t;

		bool init(ShmStruct * shmStructPtr);
		void update();
		bool process_data(uint8_t * msg);
};

Matrix f_roomba(Matrix x, FiltData_t param);
Matrix g_roomba(Matrix x, FiltData_t param);

#endif
